
#include "JointIO.h"
#include <iostream>
#include <iomanip>
#include <cassert>
#include <Windows.h>

using namespace std;
int main()
{
	//test header constants
	cout <<  "Joint " << jointNames[J_RIGHT_PALM] <<
		" has joint id " << J_RIGHT_PALM << endl ;
	assert(J_RIGHT_PALM==43);

	//initialize JointIO Object
	MapTable writeMap;
	JointIO jio(NULL,NULL,&writeMap);
	double *inBuffer = new double[jio.numJoint],
		   *outBuffer = new double[jio.numJoint];
	for(int i=0;i<jio.numJoint;i++)
		outBuffer[i]=3.5;

	//setup timer to estimate read/write time
	__int64 pfreq,startTime,endTime,cumTime=0;
	QueryPerformanceFrequency((LARGE_INTEGER*)&pfreq);

	const int testTime=100;
	for(int i=0;i<testTime;i++)
	{
		QueryPerformanceCounter((LARGE_INTEGER*)&startTime);
		jio.readJoints(inBuffer, NULL, NULL, NULL, jio.numJoint); //test pressure sensor only
		jio.writeJoints(outBuffer, outBuffer, jio.numJoint);
		QueryPerformanceCounter((LARGE_INTEGER*)&endTime);

		cumTime += (endTime - startTime);
		//Sleep(12); //simulated computation
		//for(int i=0;i<jio.numJoint;i++)
		//	cout << setprecision(1) << buffer[i] << " " ;
	}

	for(int i=0;i<jio.numJoint;i++)
		cout << i << " " << inBuffer[i] << endl;
	cout << "Time cost per I/O (ms) " << cumTime/(double)pfreq/testTime << endl;
	
	delete[] inBuffer;
	delete[] outBuffer;


	inBuffer = new double[jio.numJoint*3-6];
	jio.read(inBuffer);
	for(int i=0;i<jio.numJoint*3-6;i++)
		cout << i << " " << inBuffer[i] << endl;
	return 0;
}
